#include "nav2_util/node_utils.hpp"
#include <cmath>
#include <memory>
#include <string>
#include <fstream>
#include "nav2_core/exceptions.hpp"
#include "planner_pkg/global_navfn_planner.hpp"

namespace planner_pkg
{

    GlobalNavfnPlanner::GlobalNavfnPlanner()
        : tf_(nullptr), costmap_(nullptr)
    {
    }

    GlobalNavfnPlanner::~GlobalNavfnPlanner()
    {
        RCLCPP_INFO(
            logger_, "Destroying plugin %s of type GlobalNavfnPlanner",
            name_.c_str());
    }
    void GlobalNavfnPlanner::configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name,
        std::shared_ptr<tf2_ros::Buffer> tf,
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
    {
        tf_ = tf;
        node_ = parent.lock();
        name_ = name;
        costmap_ = costmap_ros->getCostmap();
        global_frame_ = costmap_ros->getGlobalFrameID(); // map

        clock_ = node_->get_clock();
        logger_ = node_->get_logger();
        // 参数初始化
        nav2_util::declare_parameter_if_not_declared(node_, name + ".tolerance", rclcpp::ParameterValue(0.5));
        node_->get_parameter(name_ + ".tolerance", tolerance_);
        node_->get_parameter(name_ + ".allow_unknown", allow_unknown_);
        RCLCPP_INFO(logger_, "global_frame_ = %s", global_frame_.c_str());
        // 创建导航规划
        planner_ = std::make_unique<NavFn>(
            costmap_->getSizeInCellsX(),
            costmap_->getSizeInCellsY());
    }

    void GlobalNavfnPlanner::cleanup()
    {
        RCLCPP_INFO(logger_, "正在清理类型为 GlobalNavfnPlanner 的插件 %s",
                    name_.c_str());
        planner_.reset();
    }

    void GlobalNavfnPlanner::activate()
    {
        RCLCPP_INFO(logger_, "正在激活类型为 GlobalNavfnPlanner 的插件 %s",
                    name_.c_str());
    }

    void GlobalNavfnPlanner::deactivate()
    {
        RCLCPP_INFO(logger_, "正在停用类型为 GlobalNavfnPlanner 的插件 %s",
                    name_.c_str());
    }

    nav_msgs::msg::Path
    GlobalNavfnPlanner::createPlan(const geometry_msgs::msg::PoseStamped &start,
                                   const geometry_msgs::msg::PoseStamped &goal)
    {
        // Update planner based on the new costmap size
        if (isPlannerOutOfDate())
        {
            planner_->setNavArr(
                costmap_->getSizeInCellsX(),
                costmap_->getSizeInCellsY());
        }
        nav_msgs::msg::Path global_path;
        // Corner case of the start(x,y) = goal(x,y)
        if (start.pose.position.x == goal.pose.position.x &&
            start.pose.position.y == goal.pose.position.y)
        {
            RCLCPP_WARN(logger_, "规划器失败 createPlan start(x,y) = goal(x,y)");
            return global_path;
        }

        //  使用简单的 A* 算法规划路径
        if (!makePlan(start.pose, goal.pose, global_path))
        {
            RCLCPP_WARN(logger_, "规划全局路径失败");
            throw nav2_core::PlannerException("规划全局路径失败");
        }
        // 使用 costmap 检查该条路径是否经过障碍物
        for (geometry_msgs::msg::PoseStamped pose : global_path.poses)
        {
            unsigned int mx, my; // 将点的坐标转换为栅格坐标
            if (worldToMap(pose.pose.position.x, pose.pose.position.y, mx, my))
            {
                unsigned char cost = costmap_->getCost(mx, my); // 获取对应栅格的代价值
                // 如果存在致命障碍物则抛出异常
                if (cost == nav2_costmap_2d::LETHAL_OBSTACLE)
                {
                    RCLCPP_WARN(logger_, "在(%f,%f)检测到致命障碍物，规划失败。", pose.pose.position.x, pose.pose.position.y);
                    throw nav2_core::PlannerException(
                        "无法创建目标规划: " + std::to_string(goal.pose.position.x) + "," + std::to_string(goal.pose.position.y));
                }
            }
        }
        return global_path;
    }
    /**
     * 从 start 到 goal 的路径，要求过一系列的点，这些点 pose_ 上的点，
     */
    bool GlobalNavfnPlanner::makePlan(
        const geometry_msgs::msg::Pose &start,
        const geometry_msgs::msg::Pose &goal,
        nav_msgs::msg::Path &plan)
    {
        plan.poses.clear();
        plan.header.frame_id = global_frame_;
        plan.header.stamp = clock_->now();

        double wx_start = start.position.x;
        double wy_start = start.position.y;
        unsigned int mx_start, my_start;
        if (!worldToMap(wx_start, wy_start, mx_start, my_start))
        {
            RCLCPP_WARN(logger_, "无法规划路径，机器人起始位置不在全局代价地图上。");
            return false;
        }
        // 将起始点设置为可通行
        costmap_->setCost(mx_start, my_start, nav2_costmap_2d::FREE_SPACE);
        std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
        planner_->setNavArr(
            costmap_->getSizeInCellsX(),
            costmap_->getSizeInCellsY());
        planner_->setCostmap(costmap_->getCharMap(), allow_unknown_);
        lock.unlock();
        // 起始点
        int map_start[2];
        map_start[0] = mx_start;
        map_start[1] = my_start;
        // 目标点
        double wx_goal = goal.position.x;
        double wy_goal = goal.position.y;
        unsigned int mx_goal, my_goal;
        if (!worldToMap(wx_goal, wy_goal, mx_goal, my_goal))
        {
            RCLCPP_WARN(logger_, "无法规划路径，目标点不在全局代价地图上。");
            return false;
        }
        int map_goal[2];
        map_goal[0] = mx_goal;
        map_goal[1] = my_goal;
        planner_->setStart(map_goal);
        planner_->setGoal(map_start);
        // auto start_time = std::chrono::high_resolution_clock::now();
        // 使用 A* 规划
        planner_->calcNavFnAstar();
        // 获取目标点势能
        double potential = getPointPotential(goal.position);
        if (potential >= POT_HIGH)
        {
            RCLCPP_ERROR(logger_, "规划器 无法规划路径，目标点不可通行。");
            return false;
        }

        RCLCPP_INFO(logger_, "make plan from (%.3f,%.3f) to (%.3f,%.3f)",
                    start.position.x, start.position.y, goal.position.x, goal.position.y);

        if (!getPlanFromPotential(goal, plan))
        {
            RCLCPP_ERROR(logger_, " getPlanFromPotential 规划失败");
            return false;
        }

        // 耗時
        // auto end_time = std::chrono::high_resolution_clock::now();
        // auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
        // RCLCPP_INFO(logger_, "Path found in %ld us", duration.count());

        smoothApproachToGoal(goal, plan);

        return !plan.poses.empty();
    }

    bool
    GlobalNavfnPlanner::isPlannerOutOfDate()
    {
        if (!planner_.get() ||
            planner_->nx != static_cast<int>(costmap_->getSizeInCellsX()) ||
            planner_->ny != static_cast<int>(costmap_->getSizeInCellsY()))
        {
            return true;
        }
        return false;
    }

    double
    GlobalNavfnPlanner::getPointPotential(const geometry_msgs::msg::Point &world_point)
    {
        unsigned int mx, my;
        if (!worldToMap(world_point.x, world_point.y, mx, my))
        {
            return std::numeric_limits<double>::max();
        }

        unsigned int index = my * planner_->nx + mx;
        return planner_->potarr[index];
    }

    bool
    GlobalNavfnPlanner::getPlanFromPotential(
        const geometry_msgs::msg::Pose &goal,
        nav_msgs::msg::Path &plan)
    {
        // clear the plan, just in case
        plan.poses.clear();

        // Goal should be in global frame
        double wx = goal.position.x;
        double wy = goal.position.y;

        // the potential has already been computed, so we won't update our copy of the costmap
        unsigned int mx, my;
        if (!worldToMap(wx, wy, mx, my))
        {
            RCLCPP_WARN(
                logger_,
                "The goal sent to the navfn planner is off the global costmap."
                " Planning will always fail to this goal.");
            return false;
        }

        int map_goal[2];
        map_goal[0] = mx;
        map_goal[1] = my;

        planner_->setStart(map_goal);

        const int &max_cycles = (costmap_->getSizeInCellsX() >= costmap_->getSizeInCellsY()) ? (costmap_->getSizeInCellsX() * 4) : (costmap_->getSizeInCellsY() * 4);

        int path_len = planner_->calcPath(max_cycles);
        if (path_len == 0)
        {
            return false;
        }

        auto cost = planner_->getLastPathCost();
        RCLCPP_DEBUG(
            logger_,
            "Path found, %d steps, %f cost\n", path_len, cost);

        // extract the plan
        float *x = planner_->getPathX();
        float *y = planner_->getPathY();
        int len = planner_->getPathLen();

        for (int i = len - 1; i >= 0; --i)
        {
            // convert the plan to world coordinates
            double world_x, world_y;
            mapToWorld(x[i], y[i], world_x, world_y);
            // RCLCPP_INFO(logger_, "Path point %d: x=%.2f, y=%.2f", i, world_x, world_y);
            geometry_msgs::msg::PoseStamped pose;
            pose.pose.position.x = world_x;
            pose.pose.position.y = world_y;
            pose.pose.position.z = 0.0;
            pose.pose.orientation.x = 0.0;
            pose.pose.orientation.y = 0.0;
            pose.pose.orientation.z = 0.0;
            pose.pose.orientation.w = 1.0;
            plan.poses.push_back(pose);
        }

        return !plan.poses.empty();
    }

    void
    GlobalNavfnPlanner::smoothApproachToGoal(
        const geometry_msgs::msg::Pose &goal,
        nav_msgs::msg::Path &plan)
    {
        // Replace the last pose of the computed path if it's actually further away
        // to the second to last pose than the goal pose.
        if (plan.poses.size() >= 2)
        {
            auto second_to_last_pose = plan.poses.end()[-2];
            auto last_pose = plan.poses.back();
            if (
                squared_distance(last_pose.pose, second_to_last_pose.pose) >
                squared_distance(goal, second_to_last_pose.pose))
            {
                plan.poses.back().pose = goal;
                return;
            }
        }
        geometry_msgs::msg::PoseStamped goal_copy;
        goal_copy.pose = goal;
        plan.poses.push_back(goal_copy);
    }

    bool
    GlobalNavfnPlanner::worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my)
    {
        if (wx < costmap_->getOriginX() || wy < costmap_->getOriginY())
        {
            return false;
        }

        mx = static_cast<int>(
            std::round((wx - costmap_->getOriginX()) / costmap_->getResolution()));
        my = static_cast<int>(
            std::round((wy - costmap_->getOriginY()) / costmap_->getResolution()));

        if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
        {
            return true;
        }

        RCLCPP_ERROR(
            logger_,
            "worldToMap failed: mx,my: %d,%d, size_x,size_y: %d,%d", mx, my,
            costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());

        return false;
    }

    void
    GlobalNavfnPlanner::mapToWorld(double mx, double my, double &wx, double &wy)
    {
        wx = costmap_->getOriginX() + mx * costmap_->getResolution();
        wy = costmap_->getOriginY() + my * costmap_->getResolution();
    }

} // namespace planner_pkg

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(planner_pkg::GlobalNavfnPlanner,
                       nav2_core::GlobalPlanner)